"""
舵机配置模块

该模块提供了舵机参数的配置功能，包括：
- 舵机ID配置
- 舵机偏差设置
- 角度范围限制
- 电压和温度报警设置
"""

import os
import yaml
import logging
from typing import Optional, Tuple, Dict, Any
from robot_core.protocols.serial_protocol import SerialProtocol
from robot_core.protocols.servo_protocol import ServoCommand, ServoProtocol


class ServoConfig:
    """舵机配置类"""

    def __init__(
        self, config_path: Optional[str] = None, log_level: int = logging.INFO
    ):
        """
        初始化舵机配置

        Args:
            config_path: 配置文件路径，如果为None则使用默认路径
            log_level: 日志级别
        """
        self.log_level = log_level
        self.logger = logging.getLogger(__name__)
        self.logger.setLevel(log_level)  # 设置当前模块的日志级别

        self.config = self._load_config(config_path)
        self.serial_protocol = None  # 延迟初始化
        self.servo_protocol = None  # 延迟初始化

    def _load_config(self, config_path: Optional[str] = None) -> Dict[str, Any]:
        """
        加载配置文件

        Args:
            config_path: 配置文件路径

        Returns:
            配置字典
        """
        if config_path is None:
            # 使用默认配置文件路径
            current_dir = os.path.dirname(os.path.abspath(__file__))
            config_path = os.path.join(current_dir, "..", "..", "config", "servo.yaml")

        try:
            with open(config_path, "r", encoding="utf-8") as f:
                return yaml.safe_load(f)
        except Exception as e:
            self.logger.error(f"加载配置文件失败: {e}")
            return {}

    def init_serial_protocol(
        self, port: str = "/dev/ttyS0", baudrate: int = 115200
    ) -> None:
        """
        初始化串口协议

        Args:
            port: 串口设备名
            baudrate: 波特率
        """
        self.serial_protocol = SerialProtocol(port, baudrate, log_level=self.log_level)
        self.servo_protocol = ServoProtocol(
            self.serial_protocol, log_level=self.log_level
        )

    def connect(self) -> bool:
        """连接串口"""
        is_connected = self.serial_protocol.connect()
        if not is_connected:
            self.logger.error("连接串口失败")
            self.logger.error(f"串口设备名: {self.serial_protocol.port}")
            self.logger.error(f"波特率: {self.serial_protocol.baudrate}")
            return False
        is_connected = self.servo_protocol.connect()
        if not is_connected:
            self.logger.error("连接舵机失败")
            self.logger.error(f"串口设备名: {self.serial_protocol.port}")
            self.logger.error(f"波特率: {self.serial_protocol.baudrate}")
            return False

        self.serial_protocol.port_reset()
        self.logger.info("连接串口成功")
        self.logger.info(f"串口设备名: {self.serial_protocol.port}")
        self.logger.info(f"波特率: {self.serial_protocol.baudrate}")
        return True

    def get_control_type(self) -> str:
        """获取控制方式"""
        return self.config.get("control_type", "serial")

    def get_serial_config(self) -> Dict[str, Any]:
        """获取串口配置"""
        return self.config.get("serial", {})

    def get_pwm_config(self) -> Dict[str, Any]:
        """获取PWM配置"""
        return self.config.get("pwm", {})

    def get_servo_config(self) -> Dict[str, Any]:
        """获取舵机配置"""
        return self.config.get("servo", {})

    def get_mapping(self) -> Dict[str, Any]:
        """获取舵机映射"""
        return self.config.get("mapping", {})

    def set_id(self, old_id: int, new_id: int) -> bool:
        """
        配置舵机ID号

        Args:
            old_id: 原来的ID，出厂默认为1
            new_id: 新的ID

        Returns:
            bool: 是否设置成功
        """
        return self.servo_protocol.set_servo_id(old_id, new_id)

    def read_id(self, servo_id: Optional[int] = None) -> Optional[int]:
        """
        读取串口舵机ID

        Args:
            servo_id: 舵机ID，默认为None

        Returns:
            舵机ID，如果读取失败则返回None
        """
        self.serial_protocol.port_reset()
        if servo_id is None:  # 总线上只能有一个舵机
            self.servo_protocol.read_command(0xFE, ServoCommand.ID_READ)
        else:
            self.servo_protocol.read_command(servo_id, ServoCommand.ID_READ)

        response = self.servo_protocol.get_response(ServoCommand.ID_READ)
        if response is not None:
            return response
        self.logger.warning("Only one servo can be configured at a time")
        return None

    def stop(self, servo_id: Optional[int] = None) -> bool:
        """
        停止舵机运行

        Args:
            servo_id: 舵机ID

        Returns:
            bool: 是否停止成功
        """
        return self.servo_protocol.write_command(
            servo_id if servo_id is not None else 0xFE, ServoCommand.MOVE_STOP, []
        )

    def set_deviation(self, servo_id: int, deviation: int = 0) -> bool:
        """
        配置舵机偏差，掉电保护

        Args:
            servo_id: 舵机ID
            deviation: 偏差值

        Returns:
            bool: 是否设置成功
        """
        return self.servo_protocol.set_servo_offset(servo_id, deviation)

    def read_deviation(self, servo_id: int) -> Optional[int]:
        """
        读取舵机偏差值

        Args:
            servo_id: 舵机ID

        Returns:
            偏差值，如果读取失败则返回None
        """
        return self.servo_protocol.get_servo_offset(servo_id)

    def set_angle_limit(self, servo_id: int, low: int, high: int) -> bool:
        """
        设置舵机转动范围

        Args:
            servo_id: 舵机ID
            low: 最小角度
            high: 最大角度

        Returns:
            bool: 是否设置成功
        """
        return self.servo_protocol.set_servo_angle_limit(servo_id, low, high)

    def read_angle_limit(self, servo_id: int) -> Optional[Tuple[int, int]]:
        """
        读取舵机转动范围

        Args:
            servo_id: 舵机ID

        Returns:
            元组(最小角度, 最大角度)，如果读取失败则返回None
        """
        return self.servo_protocol.get_servo_angle_limit(servo_id)

    def set_vin_limit(self, servo_id: int, low: int, high: int) -> bool:
        """
        设置舵机电压报警范围

        Args:
            servo_id: 舵机ID
            low: 最小电压
            high: 最大电压

        Returns:
            bool: 是否设置成功
        """
        return self.servo_protocol.write_command(
            servo_id, ServoCommand.VIN_LIMIT_WRITE, [low, high]
        )

    def read_vin_limit(self, servo_id: int) -> Optional[Tuple[int, int]]:
        """
        读取舵机电压报警范围

        Args:
            servo_id: 舵机ID

        Returns:
            元组(最小电压, 最大电压)，如果读取失败则返回None
        """
        if self.servo_protocol.read_command(servo_id, ServoCommand.VIN_LIMIT_READ):
            response = self.servo_protocol.get_response(ServoCommand.VIN_LIMIT_READ)
            if isinstance(response, tuple) and len(response) == 2:
                return response
        return None

    def set_max_temp(self, servo_id: int, max_temp: int) -> bool:
        """
        设置舵机最高温度报警

        Args:
            servo_id: 舵机ID
            max_temp: 最高温度

        Returns:
            bool: 是否设置成功
        """
        return self.servo_protocol.write_command(
            servo_id, ServoCommand.TEMP_MAX_LIMIT_WRITE, [max_temp]
        )

    def read_temp_limit(self, servo_id: int) -> Optional[int]:
        """
        读取舵机温度报警范围

        Args:
            servo_id: 舵机ID

        Returns:
            最高温度，如果读取失败则返回None
        """
        if self.servo_protocol.read_command(servo_id, ServoCommand.TEMP_MAX_LIMIT_READ):
            return self.servo_protocol.get_response(ServoCommand.TEMP_MAX_LIMIT_READ)
        return None

    def read_position(self, servo_id: int) -> Optional[int]:
        """
        读取舵机当前位置

        Args:
            servo_id: 舵机ID

        Returns:
            当前位置，如果读取失败则返回None
        """
        return self.servo_protocol.get_servo_angle(servo_id)

    def read_temperature(self, servo_id: int) -> Optional[int]:
        """
        读取舵机温度

        Args:
            servo_id: 舵机ID

        Returns:
            当前温度，如果读取失败则返回None
        """
        return self.servo_protocol.get_servo_temperature(servo_id)

    def read_voltage(self, servo_id: int) -> Optional[int]:
        """
        读取舵机电压

        Args:
            servo_id: 舵机ID

        Returns:
            当前电压，如果读取失败则返回None
        """
        return self.servo_protocol.get_servo_voltage(servo_id)

    def show_servo_state(self) -> Optional[int]:
        """
        显示舵机状态信息

        Returns:
            舵机ID，如果读取失败则返回None
        """
        servo_id = self.read_id()
        self.serial_protocol.port_reset()
        if servo_id is not None:
            self.logger.info(f"当前的舵机ID是：{servo_id}")

            pos = self.read_position(servo_id)
            self.serial_protocol.port_reset()
            if pos is not None:
                self.logger.info(f"当前的舵机角度：{pos}")

            temp = self.read_temperature(servo_id)
            self.serial_protocol.port_reset()
            if temp is not None:
                self.logger.info(f"当前的舵机温度：{temp}°")

            vin = self.read_voltage(servo_id)
            self.serial_protocol.port_reset()
            if vin is not None:
                self.logger.info(f"当前的舵机电压：{vin}mv")

            deviation = self.read_deviation(servo_id)
            self.serial_protocol.port_reset()
            if deviation is not None:
                self.logger.info(f"当前的舵机偏差：{deviation}")

            angle_limit = self.read_angle_limit(servo_id)
            self.serial_protocol.port_reset()
            if angle_limit is not None:
                self.logger.info(
                    f"当前的舵机可控角度为{angle_limit[0]}-{angle_limit[1]}"
                )

            vin_limit = self.read_vin_limit(servo_id)
            self.serial_protocol.port_reset()
            if vin_limit is not None:
                self.logger.info(
                    f"当前的舵机报警电压为{vin_limit[0]}mv-{vin_limit[1]}mv"
                )

            temp_limit = self.read_temp_limit(servo_id)
            self.serial_protocol.port_reset()
            if temp_limit is not None:
                self.logger.info(f"当前的舵机报警温度为50°-{temp_limit}°")

        return servo_id

    def close(self) -> None:
        """关闭串口连接"""
        if self.servo_protocol:
            self.servo_protocol.disconnect()


# 创建全局配置实例
servo_config = ServoConfig()
